from numpy import *
import pdb
from numpy.linalg import *
from math import *
class sensor_ball(object):
	"""This sensor gives you the relative distance and angle to the ball or ball carrier.  Returns 0 for distance and angle if the carrier requests it."""
	def __init__(self, agent_num, world_model, soccer_game_model,max_dist=120):
		self.agent_num=agent_num
		self.wm=world_model
		self.sgm=soccer_game_model
		self.max_dist=max_dist
	def sense(self):
			holder=self.sgm.ball_holder
			target_heading=0
			target_magnitude=0
			if holder==-1:  # Ball is loose, and thus we get the position to the ball object
				#pdb.set_trace()
				balls=filter(lambda x: x.object_type==0,self.wm.objects.itervalues())
				pos=array(balls[0].pos)
				cur_pos=array(self.wm.objects[self.agent_num].pos)
				cur_heading=self.wm.objects[self.agent_num].heading
				diff_vec=pos-cur_pos
				target_magnitude=min(norm(diff_vec),self.max_dist)
				target_heading=((atan2(diff_vec[1],diff_vec[0])-cur_heading) % (2*pi))-pi
			else:
				ball_holder=filter(lambda x: x.object_num==holder,self.wm.objects.itervalues())
				pos=array(ball_holder[0].pos)
				cur_pos=array(self.wm.objects[self.agent_num].pos)
				cur_heading=self.wm.objects[self.agent_num].heading
				diff_vec=pos-cur_pos
				target_magnitude=min(norm(diff_vec),self.max_dist)
				target_heading=((atan2(diff_vec[1],diff_vec[0])-cur_heading) % (2*pi))-pi

			return target_magnitude,target_heading
